/*******************************************************************************
*
*   Copyright ©2009 Alexandru Marin
*
*   This file is part of nwave/pMRI C library.
*
*   This project started as an MSc degree project during a
*   stage at the Paris-Est university, under joint supervision of prof.
*   Bogdan Ionescu (Image Analysis and Processing Laboratory, "Politehica"
*   University of Bucharest), Caroline Chaux and Jean-Christophe Pesquet
*   (Laboratoire d'Informatique, Université Paris-Est, Institut Gaspard Monge).
*
*   nwave/pMRI C libray is free software: you can redistribute it and/or modify
*   it under the terms of the GNU General Public License as published by
*   the Free Software Foundation, either version 2 of the License, or
*   (at your option) any later version.
*
*   nwave/pMRI C libray is distributed in the hope that it will be useful,
*   but WITHOUT ANY WARRANTY; without even the implied warranty of
*   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
*   GNU General Public License for more details.
*
*   You should have received a copy of the GNU General Public License
*   along with nwave/pMRI C library.  If not, see <http://www.gnu.org/licenses/>.
*
******************************************************************************/


//#define MRI_NOISE_USE_MATLAB

#include "mri_noise.h"
#include "mri_utils.h"
#include <assert.h>

mri_noise_cov_t* mri_noise_cov_read(FILE* f) {

	mri_matrix_descriptor_t desc;

	mri_noise_cov_t* result = (mri_noise_cov_t*) malloc(sizeof(mri_noise_cov_t));
	assert(result != NULL);

	// Note that the covariance is always a square matrix
	result->data = mri_read_real_matrix(f, &desc);
	assert(desc.ndims == 2);
	assert(desc.dims[0] == desc.dims[1]);
	result->size = desc.dims[0];

	return result;
}

mri_noise_cov_t* mri_noise_cov_load(char* name) {

	char* fullpath = mri_strcat(3, mri_get_path(), name, ".csv");

	FILE* f = fopen(fullpath, "r");
	assert(f!=NULL);

	mri_noise_cov_t* result = mri_noise_cov_read(f);

	fclose(f);
	free(fullpath);

	return result;
}

mri_noise_cov_t* mri_noise_cov_gen_diag(int size, float sigma) {

	int i;

	// Allocate result
	mri_noise_cov_t* result = (mri_noise_cov_t*) malloc(sizeof(mri_noise_cov_t));
	result->data = (float*) malloc(size * size * sizeof(float));

	// Fill in with data...
	result->size = size;

	for (i = 0; i < result->size; i++)
		result->data[i * (result->size + 1)] = sigma * sigma;

	return result;
}

void mri_noise_cov_free(mri_noise_cov_t* noise_cov) {
	free(noise_cov->data);
	free(noise_cov);
}

complex float* mri_noise_cov_get_instance(mri_noise_cov_t* noise_cov) {

	complex float* result = (complex float*) malloc(noise_cov->size * sizeof(complex float));
	assert(result != NULL);

	complex float* gaussian_noise = mri_noise_gen_gaussian(noise_cov->size, 0, 1);

	int i, j;
	float coeff;

	for (i = 0; i < noise_cov->size; i++) {

		result[i] = 0*I;

		for (j = 0; j < noise_cov->size; j++) {

			coeff = sqrt(noise_cov->data[i * noise_cov->size + j] / 2);

			result[i] += coeff * gaussian_noise[j];
		}
	}

	free(gaussian_noise);

	return result;
}

void mri_noise_cov_fill_instance(complex float* noise_vector, mri_noise_cov_t* noise_cov) {

	complex float* gaussian_noise = mri_noise_gen_gaussian(noise_cov->size, 0, 1);

	int i, j;
	float coeff;

	for (i = 0; i < noise_cov->size; i++) {

		noise_vector[i] = 0*I;

		for (j = 0; j < noise_cov->size; j++) {

			coeff = sqrt(noise_cov->data[i * noise_cov->size + j] / 2);

			noise_vector[i] += coeff * gaussian_noise[j];
		}
	}

	free(gaussian_noise);
}


complex float* mri_noise_gen_gaussian(int size, float miu, float sigma) {

	complex float* result;
	result = (complex float*) malloc(size * sizeof(complex float));
	assert(result != NULL);

	int i;

#ifdef MRI_NOISE_USE_MATLAB
	// NOTE: generated in this order (first reals, then imag)
	// in order to match matlab sample mri noising program generation style
	for (i = 0; i < size; i++)
		result[i] = mri_noise_randn(miu, sigma);
	for (i = 0; i < size; i++)
		result[i] += I * mri_noise_randn(miu, sigma);
#else
	for (i = 0; i < size; i++)
		result[i] = mri_noise_randn(miu, sigma) + I * mri_noise_randn(miu, sigma);
#endif

	return result;
}

static FILE* f = NULL;

/**
 * Returns a random number from a gaussian sequence
 * as generated by matlab with seed 0 (for testing purposes)
 * Use carefully (1M numbers available)
 */
float mri_noise_matlab_randn() {

	if (f == NULL) {
		printf("* * * Initializing Matlab random number generator...\n");
		if ((f = fopen("matlab_first_1M_randoms.txt", "r")) == NULL) {
			printf("Could not initialize matlab random number generator!\nThe matlab_first_1M_randoms.txt file could not be found...\n");
			exit(0);
		}
	}

	float result;

	if (fscanf(f, "%f", &result) == EOF) {
		printf("Matlab random number generator reached the end!!");
		exit(0);
	}

	return result;
}

static int ncalls = 0;

int mri_noise_ncalls() {
	return ncalls;
}

float mri_noise_randn(float miu, float sigma) {

	ncalls++;

	// If this flag is set, then we should use the random numbers from matlab
#ifdef MRI_NOISE_USE_MATLAB
	return miu + mri_noise_matlab_randn() * sigma;
#else
	// Original code from the GNU Scientific Library
	/* Ratio method (Kinderman-Monahan); see Knuth v2, 3rd ed, p130.
	 * K+M, ACM Trans Math Software 3 (1977) 257-260.
	 *
	 * [Added by Charles Karney] This is an implementation of Leva's
	 * modifications to the original K+M method; see:
	 * J. L. Leva, ACM Trans Math Software 18 (1992) 449-453 and 454-455. */
	float u, v, x, y, Q;
	const float s = 0.449871; /* Constants from Leva */
	const float t = -0.386595;
	const float a = 0.19600;
	const float b = 0.25472;
	const float r1 = 0.27597;
	const float r2 = 0.27846;

	do /* This loop is executed 1.369 times on average  */
	{
		/* Generate a point P = (u, v) uniform in a rectangle enclosing
		 the K+M region v^2 <= - 4 u^2 log(u). */

		/* u in (0, 1] to avoid singularity at u = 0 */
		u = 1 - mri_noise_randu();

		/* v is in the asymmetric interval [-0.5, 0.5).  However v = -0.5
		 is rejected in the last part of the while clause.  The
		 resulting normal deviate is strictly symmetric about 0
		 (provided that v is symmetric once v = -0.5 is excluded). */
		v = mri_noise_randu() - 0.5;

		/* Constant 1.7156 > sqrt(8/e) (for accuracy); but not by too
		 much (for efficiency). */
		v *= 1.7156;

		/* Compute Leva's quadratic form Q */
		x = u - s;
		y = fabs(v) - t;
		Q = x * x + y * (a * y - b * x);

		/* Accept P if Q < r1 (Leva) */
		/* Reject P if Q > r2 (Leva) */
		/* Accept if v^2 <= -4 u^2 log(u) (K+M) */
		/* This final test is executed 0.012 times on average. */
	} while (Q >= r1 && (Q > r2 || v * v > -4 * u * u * log(u)));

	return miu + sigma * (v / u);
#endif
}

float mri_noise_randu() {
	return (rand()) / ((float) RAND_MAX + 1);
}

